#ifndef SHERIFFOS2_DAMIAO_MOTOR_H
#define SHERIFFOS2_DAMIAO_MOTOR_H

#include <algorithm/math.h>
#include <connectivity/can.h>
#include <clock.h>
#include <connection_guard.h>

constexpr float kRadius2Angle = 180.0F / 3.1415926F;
constexpr float kRadius2RPM = 9.5492965855137201461330258023509F;  ///< 60 / (2 * PI)
constexpr float kAngle2Radius = 3.1415926F / 180.0F;
constexpr float kRPM2Radius = 0.10471975511965977461542144610932F;  ///< 2 * PI / 60

enum class DamiaoMotorState : uint8_t { kEnable = 0, kDisable, kLost, kError };
enum class DamiaoMotorErrorCode : uint8_t {
    kDisabled = 0x0,
    kEnabled = 0x01,
    kOverVoltage = 0x8,
    kUnderVoltage = 0x9,
    kOverCurrent = 0xA,
    kMOS_OverTemperature = 0xB,
    kRotor_OverTemperature = 0xC,
    kConnectLost = 0xD,
    kEncoderError = 0xE
};

namespace detail {
[[clang::always_inline]] static float uint2float(int const num, float const min, float const max, int const bits) {
    float const span = max - min;
    float res = static_cast<float>(num) * span / static_cast<float>((1 << bits) - 1) + min;
    LimitMaxMin(res, max, min);
    return res;
}
[[clang::always_inline]] static int float2uint(float num, float const min, float const max, int const bits) {
    LimitMaxMin(num, max, min);
    float const span = max - min;
    return static_cast<int>((num - min) * static_cast<float>((1 << bits) - 1) / span);
}
}  // namespace detail

struct DamiaoEncoderData {
    DamiaoMotorErrorCode errorCode = {};        ///< 电机错误码
    uint16_t kp = 0;                            ///< 电机MIT参数
    uint16_t ki = 0;                            ///< 电机MIT参数
    units::angle::degree_t rawAngle{};          ///< (单位：角度)电机编码器返回角0-360（不含角度偏移）
    units::angle::degree_t limitedAngle{};      ///< 编码器角度-180~180（含角度偏移）
    units::angle::degree_t lastLimitedAngle{};  ///< 上次角度(含角度偏移)
    units::angular_velocity::revolutions_per_minute_t rawSpeed{};    ///< (单位：rpm)电机编码器返回速度
    float torque = 0;                                                ///< (单位：NM)电机编码器返回力矩
    uint8_t mosTemp = 0;                                             ///< (单位：摄氏度)驱动上MOS的平均温度
    uint8_t rotorTemp = 0;                                           ///< (单位：摄氏度)转子温度
    int32_t roundCount = 0;                                          ///< 电机旋转圈数(含角度偏移)
    units::angle::degree_t shaftContinuousAngle{};                   ///< (单位：角度)电机输出轴连续角度(含角度偏移)
    units::angular_velocity::revolutions_per_minute_t shaftSpeed{};  ///< (单位：rpm)电机输出轴速度};
};

class DamiaoEncoder {
protected:
    DamiaoEncoderData data_{};
    float gearRatio_{};                                         ///< 减速比
    units::angle::degree_t angleOffset_{};                      ///< (单位：角度)电机角度偏移
    units::angle::radian_t angleMax_{};                         ///< (单位：rad)电机最大角度
    units::angular_velocity::radians_per_second_t speedMax_{};  ///< (单位：rad/s)电机最大速度
    float torqueMax_{};                                         ///< 电机最大力矩
    units::time::second_t lastUpdateTime_{};                    ///< (单位：毫秒) 上次更新时间

public:
    DamiaoEncoder() = default;
    explicit DamiaoEncoder(float const gearRatio, units::angle::degree_t const angleOffset,
                           float const angleMax = 12.5F, float const speedMax = 30.0F, float const torqueMax = 10.0F)
        : gearRatio_(gearRatio),
          angleOffset_(angleOffset),
          angleMax_(angleMax),
          speedMax_(speedMax),
          torqueMax_(torqueMax) {}

    ~DamiaoEncoder() = default;

    void decode(uint8_t const* data) {
        lastUpdateTime_ = os::getTime();
        data_.lastLimitedAngle = data_.limitedAngle;

        data_.errorCode = static_cast<DamiaoMotorErrorCode>(data[0] >> 4);
        data_.rawAngle = units::make_unit<units::angle::degree_t>(
            detail::uint2float(data[1] << 8 | data[2], -angleMax_.to<float>(), angleMax_.to<float>(), 16) *
            kRadius2Angle);
        data_.rawSpeed = units::make_unit<units::angular_velocity::revolutions_per_second_t>(
            detail::uint2float(data[3] << 4 | data[4] >> 4, -speedMax_.to<float>(), speedMax_.to<float>(), 12) *
            kRadius2RPM);
        data_.torque = detail::uint2float((data[4] & 0x0F) << 8 | data[5], -torqueMax_, torqueMax_, 12);
        data_.mosTemp = static_cast<uint8_t>(data[6]);
        data_.rotorTemp = static_cast<uint8_t>(data[7]);

        data_.limitedAngle = data_.rawAngle + angleOffset_;
        while (data_.lastLimitedAngle > 180_deg) data_.lastLimitedAngle -= 180_deg;
        while (data_.lastLimitedAngle < -180_deg) data_.lastLimitedAngle += 180_deg;

        if (auto const DiffAngle = data_.limitedAngle - data_.lastLimitedAngle;
            DiffAngle < -angleMax_ * kRadius2Angle) {
            data_.roundCount++;
        } else if (DiffAngle > angleMax_ * kRadius2Angle) {
            data_.roundCount--;
        }
        data_.shaftContinuousAngle = data_.limitedAngle + 2.f * data_.roundCount * angleMax_ * kRadius2Angle;

        data_.shaftSpeed = data_.rawSpeed / gearRatio_;
    }

    /**
     * @brief 获取数据
     */
    [[nodiscard]] DamiaoEncoderData const& getData() const { return data_; }

    [[nodiscard]] auto getAngleMax() const { return angleMax_; }

    [[nodiscard]] auto getSpeedMax() const { return speedMax_; }

    [[nodiscard]] auto getTorqueMax() const { return torqueMax_; }

    [[nodiscard]] auto getLastUpdateTime() const { return lastUpdateTime_; }
};

/**
 * @brief 概念约束Can类型
 */

template <os::CanPort Can>
class DamiaoMotor {
    DamiaoMotorState state_{DamiaoMotorState::kLost};
    uint32_t id_;
    FDCAN_TxHeaderTypeDef txHeader_;
    uint32_t masterId_;

public:
    DamiaoEncoder encoder_;
    DamiaoMotor() = delete;

    /**
     * @brief 达妙电机构造函数
     * @param ID 电机ID
     * @param MST_ID 电机反馈数据帧ID
     * @param GearRatio 减速比(不含电机减速箱)
     * @param AngleOffset 角度偏移
     * @param can
     * @param AngleMax
     * @param SpeedMax
     * @param TorqueMax
     * @param UseFDCan
     */
    explicit DamiaoMotor(uint32_t const ID, uint32_t const MST_ID, float const GearRatio,
                         units::angle::degree_t const AngleOffset, float const AngleMax,
                         float const SpeedMax, float const TorqueMax, bool const UseFDCan = false)
        : id_(ID),
          txHeader_({
              .Identifier = id_,
              .IdType = FDCAN_STANDARD_ID,
              .TxFrameType = FDCAN_DATA_FRAME,
              .DataLength = FDCAN_DLC_BYTES_8,
              .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
              .BitRateSwitch = FDCAN_BRS_OFF,
              .FDFormat = FDCAN_CLASSIC_CAN,
              .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
              .MessageMarker = 0,
          }),
          masterId_(MST_ID),
          encoder_(GearRatio, AngleOffset, AngleMax, SpeedMax, TorqueMax) {
        UseFDCan ? txHeader_ =
                       {
                           .Identifier = id_,
                           .IdType = FDCAN_STANDARD_ID,
                           .TxFrameType = FDCAN_DATA_FRAME,
                           .DataLength = FDCAN_DLC_BYTES_8,
                           .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
                           .BitRateSwitch = FDCAN_BRS_ON,
                           .FDFormat = FDCAN_FD_CAN,
                           .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
                           .MessageMarker = 0,
                       }
                 : txHeader_ = {
                       .Identifier = id_,
                       .IdType = FDCAN_STANDARD_ID,
                       .TxFrameType = FDCAN_DATA_FRAME,
                       .DataLength = FDCAN_DLC_BYTES_8,
                       .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
                       .BitRateSwitch = FDCAN_BRS_OFF,
                       .FDFormat = FDCAN_CLASSIC_CAN,
                       .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
                       .MessageMarker = 0,
                   };
        os::ConnectionGuard::addGuard([this] { checkMotorState(); });
    }
    ~DamiaoMotor() = default;

    void Enable() const {
        uint8_t data[8] = {};
        data[0] = 0xFF;
        data[1] = 0xFF;
        data[2] = 0xFF;
        data[3] = 0xFF;
        data[4] = 0xFF;
        data[5] = 0xFF;
        data[6] = 0xFF;
        data[7] = 0xFC;
        Can::instance().transmit(txHeader_, data);
    }

    void Disable() {
        uint8_t data[8] = {};
        data[0] = 0xFF;
        data[1] = 0xFF;
        data[2] = 0xFF;
        data[3] = 0xFF;
        data[4] = 0xFF;
        data[5] = 0xFF;
        data[6] = 0xFF;
        data[7] = 0xFD;
        txHeader_.Identifier = id_;
        Can::instance().transmit(txHeader_, data);
    }

    void ClearError() {
        uint8_t data[8] = {};
        data[0] = 0xFF;
        data[1] = 0xFF;
        data[2] = 0xFF;
        data[3] = 0xFF;
        data[4] = 0xFF;
        data[5] = 0xFF;
        data[6] = 0xFF;
        data[7] = 0xFB;
        txHeader_.Identifier = id_;
        Can::instance().transmit(txHeader_, data);
    }

    /**
     * @brief 从寄存器读参数
     * @param address 寄存器地址
     */
    void ReadRegister(uint8_t const address) {
        uint8_t data[8] = {};
        data[0] = id_ & 0xFF;
        data[1] = id_ >> 8 & 0x07;
        data[2] = 0x33;
        data[3] = address;
        txHeader_.Identifier = 0x7FF;
        Can::instance().transmit(txHeader_, data);
    }

    /**
     * @brief 向寄存器写参数
     * @param address 寄存器地址
     * @param data 低位为data[0]，最高位为data[7]
     * @note 写寄存器数据立即生效，但无法进行存储，掉电后丢失，需要发送存储参数的命令，将修改的参数全部写入片内。
     */
    void WriteRegister(uint8_t const address, uint8_t const data[4]) {
        uint8_t tx_data[8] = {};
        tx_data[0] = id_ & 0xFF;
        tx_data[1] = id_ >> 8 & 0x07;
        tx_data[2] = 0x55;
        tx_data[3] = address;
        tx_data[4] = data[0];
        tx_data[5] = data[1];
        tx_data[6] = data[2];
        tx_data[7] = data[3];
        txHeader_.Identifier = 0x7FF;
        Can::instance().transmit(txHeader_, tx_data);
    }

    /**
     * @brief 存储参数
     * @note 注意: 1.存储参数只在失能模式下生效。
     *            2.存储参数时将一次性保留全部参数。
     *            3.该操作将参数写入片内flash中，每次操作时间最大为30ms，请注意留足足够的时间。
     *            4.flash擦写次数约1万次，请不要频繁发送“存储参数”指令。
     */
    void SaveAllRegister() {
        uint8_t data[8] = {};
        data[0] = id_ & 0xFF;
        data[1] = id_ >> 8 & 0x07;
        data[2] = 0xAA;
        data[3] = 0x01;
        txHeader_.Identifier = 0x7FF;
        txHeader_.DataLength = FDCAN_DLC_BYTES_4;
        Can::instance().transmit(txHeader_, data);
        txHeader_.DataLength = FDCAN_DLC_BYTES_8;
    }

    /**
     * @brief MIT模式发送
     * @param Angle 角度期望(单位：度)
     * @param Speed 速度期望(单位：rpm)
     * @param Kp 角度增益
     * @param Kd 速度增益
     * @param Torque 力矩期望(单位：Nm)
     * @note 当使用角度期望时Kd不可以为0
     */
    void SetSendMIT(float Angle, float Speed, float const Kp, float const Kd, float const Torque) {
        uint8_t data[8] = {};
        Angle *= kAngle2Radius;
        Speed *= kRPM2Radius;
        uint16_t const pos_tmp =
            detail::float2uint(Angle, -encoder_.getAngleMax().to<float>(), encoder_.getAngleMax().to<float>(), 16);
        uint16_t const speed_tmp =
            detail::float2uint(Speed, -encoder_.getSpeedMax().to<float>(), encoder_.getSpeedMax().to<float>(), 12);
        uint16_t const kp_tmp = detail::float2uint(Kp, 0.f, 500.f, 12);
        uint16_t const kd_tmp = detail::float2uint(Kd, 0.f, 5.f, 12);
        uint16_t const torque_tmp = detail::float2uint(Torque, -encoder_.getTorqueMax(), encoder_.getTorqueMax(), 12);
        data[0] = pos_tmp >> 8;
        data[1] = pos_tmp;
        data[2] = speed_tmp >> 4;
        data[3] = (speed_tmp & 0xF) << 4 | kp_tmp >> 8;
        data[4] = kp_tmp;
        data[5] = kd_tmp >> 4;
        data[6] = (kd_tmp & 0xF) << 4 | torque_tmp >> 8;
        data[7] = static_cast<uint8_t>(torque_tmp & 0xFF);
        txHeader_.Identifier = id_;
        Can::instance().transmit(txHeader_, data);
    }

    /**
     * @brief 位置速度模式发送
     * @param Angle 角度期望(单位：度)
     * @param Speed 最大速度(单位：rpm)
     */
    void SetSendAngleSpeed(float Angle, float Speed) {
        Angle *= kAngle2Radius;
        Speed *= kRPM2Radius;
        uint8_t data[8] = {};
        memcpy(data, &Angle, 4);
        memcpy(data + 4, &Speed, 4);
        txHeader_.Identifier = id_ + 0x100;
        Can::instance().transmit(txHeader_, data);
    }

    /**
     * @brief 速度模式发送
     * @param Speed 速度期望(单位：rpm)
     */
    void SetSendSpeed(float Speed) {
        Speed *= kRPM2Radius;
        uint8_t data[8] = {};
        memcpy(data, &Speed, 4);
        txHeader_.Identifier = id_ + 0x200;
        txHeader_.DataLength = FDCAN_DLC_BYTES_4;
        Can::instance().transmit(txHeader_, data);
        txHeader_.DataLength = FDCAN_DLC_BYTES_8;
    }

    void checkMotorState() {
        if (os::getTime() - encoder_.getLastUpdateTime() > 200_ms) {
            state_ = DamiaoMotorState::kLost;
        }
    }

    [[nodiscard]] DamiaoMotorState GetState() const { return state_; }
    [[nodiscard]] bool IsEnable() const { return state_ == DamiaoMotorState::kEnable; }
    [[nodiscard]] bool IsDisable() const { return state_ == DamiaoMotorState::kDisable; }
    [[nodiscard]] bool IsError() const { return state_ == DamiaoMotorState::kError; }
    [[nodiscard]] bool IsLost() const { return state_ == DamiaoMotorState::kLost; }

    void Decode(uint16_t const ID, uint8_t const* data) {
        if (ID == masterId_) {
            encoder_.decode(data);
            switch (encoder_.getData().errorCode) {
                case DamiaoMotorErrorCode::kDisabled:
                    state_ = DamiaoMotorState::kDisable;
                    break;
                case DamiaoMotorErrorCode::kEnabled:
                    state_ = DamiaoMotorState::kEnable;
                    break;
                default:
                    state_ = DamiaoMotorState::kError;
                    break;
            }
        }
    }
};

#endif  // SHERIFFOS2_DAMIAO_MOTOR_H
